#include "ag_config.h"
#include <regex>
#include "jkutil/iniparser.h"
#include "config.h"
#include "axisgroup.h"
#include "force_control.h"

using axisgroup::AgConfig;
using axisgroup::ConfigItem;
using std::make_shared;

static int get_tcp1d(Util::IniParser* p, const std::string& path, AgConfig::Tcp1D* value, AgConfig::Tcp1D def, double min, double max)
{
    std::vector<double> v;
    int ret = p->get(path, v, std::vector<double>({def.trans, def.ori}), min, max);
    value->trans = v[0];
    value->ori = v[1];
    return ret;
}
static int get_robjoint(Util::IniParser* p, const std::string& path, RobJointVal* value, double dv, double min, double max)
{
    std::vector<double> v;
    std::vector<double> def;
    for (size_t i = 0; i < RobJointVal::MAX_AXIS; i++) { def.push_back(dv); }
    int ret = p->get(path, v, def, min, max);
    *value = v;
    return ret;
}
class ConfigInt32Type : public ConfigItem
{
public:
    ConfigInt32Type(const char* path, int32_t* value) : ConfigItem(path, (void*)value) {}
    virtual void set_value(Util::IniParser* p) { p->set(path_, *(int32_t*)value_); }
    virtual void get_value(Util::IniParser* p)
    {
        int32_t v;
        p->get(path_, v, 0, INT32_MIN, INT32_MAX);
        *(int32_t*)value_ = v;
    }
};

class ConfigTraj : public ConfigItem
{
public:
    ConfigTraj(AgConfig::Traj* value) : ConfigItem("TRAJ.", (void*)value){};
    virtual void set_value(Util::IniParser* p)
    {
        auto traj = (AgConfig::Traj*)value_;
        p->set(path_ + "TCP_MAX_VEL", std::vector<double>({traj->vel.trans, traj->vel.ori}));
        p->set(path_ + "TCP_MAX_ACC", std::vector<double>({traj->acc.trans, traj->acc.ori}));
        p->set(path_ + "TCP_MAX_JERK", std::vector<double>({traj->jerk.trans, traj->jerk.ori}));
    }
    virtual void get_value(Util::IniParser* p)
    {
        auto traj = (AgConfig::Traj*)value_;
        get_tcp1d(p, path_ + "TCP_MAX_VEL", &traj->vel, {2000, 180}, 0, 20000);
        get_tcp1d(p, path_ + "TCP_MAX_ACC", &traj->acc, {3500, 360}, 0, 20000);
        get_tcp1d(p, path_ + "TCP_MAX_JERK", &traj->jerk, {0, 0}, 0, 500000);
    }
};

class ConfigJog : public ConfigItem
{
public:
    ConfigJog(AgConfig::Jog* value) : ConfigItem("JOG.", (void*)value){};
    void set_value(Util::IniParser* p)
    {
        auto jog = (AgConfig::Jog*)value_;
        p->set(path_ + "JOG_MAX_JOINT_VEL", jog->j_vel.vector_value());
        p->set(path_ + "JOG_MAX_JOINT_ACC", jog->j_acc.vector_value());
        p->set(path_ + "JOG_MAX_JOINT_JERK", jog->j_jerk);

        p->set(path_ + "JOG_MAX_TRAJ_VEL", std::vector<double>({jog->vel.trans, jog->vel.ori}));
        p->set(path_ + "JOG_MAX_TRAJ_ACC", std::vector<double>({jog->acc.trans, jog->acc.ori}));
        p->set(path_ + "JOG_MAX_TRAJ_JERK", std::vector<double>({jog->jerk.trans, jog->jerk.ori}));

        p->set(path_ + "POS_JOG_ACC", jog->pos_jog_acc);
        p->set(path_ + "ORI_JOG_ACC", jog->ori_jog_acc);
    }
    void get_value(Util::IniParser* p)
    {
        auto jog = (AgConfig::Jog*)value_;
        get_robjoint(p, path_ + "JOG_MAX_JOINT_VEL", &jog->j_vel, 180, 0, 1000);
        get_robjoint(p, path_ + "JOG_MAX_JOINT_ACC", &jog->j_acc, 720, 0, 10000);
        p->get(path_ + "JOG_MAX_JOINT_JERK", jog->j_jerk, 0, 0, 500000);

        get_tcp1d(p, path_ + "JOG_MAX_TRAJ_VEL", &jog->vel, {250, 180}, 0, 20000);
        get_tcp1d(p, path_ + "JOG_MAX_TRAJ_ACC", &jog->acc, {100, 360}, 0, 20000);
        get_tcp1d(p, path_ + "JOG_MAX_TRAJ_JERK", &jog->jerk, {0, 0}, 0, 500000);

        std::vector<double> default_pos = {50.0, 50.0, 10.0, 10.0, 5.0, 5.0, 5.0};
        std::vector<double> default_ori = {20.0, 20.0, 10.0, 10.0, 5.0, 5.0, 5.0};
        std::vector<double> v;
        p->get(path_ + "POS_JOG_ACC", v, default_pos, 0, 100);
        jog->pos_jog_acc = v;
        p->get(path_ + "ORI_JOG_ACC", v, default_ori, 0, 100);
        jog->ori_jog_acc = v;
    }
};

class ConfigServoMove : public ConfigItem
{
public:
    ConfigServoMove(AgConfig::ServoMove* value) : ConfigItem("SERVOMOVE.", (void*)value){};
    void set_value(Util::IniParser* p)
    {
        auto servo = (AgConfig::ServoMove*)value_;
        p->set(path_ + "FILTER_TYPE", servo->filter_type);
        set_mmf(p, servo);
        set_nlf(p, servo);
        set_lpf(p, servo);
    }
    void get_value(Util::IniParser* p)
    {
        auto servo = (AgConfig::ServoMove*)value_;
        p->get(path_ + "FILTER_TYPE", servo->filter_type, 4, 0, 10);
        get_mmf(p, servo);
        get_nlf(p, servo);
        get_lpf(p, servo);
    }
    void set_mmf(Util::IniParser* p, AgConfig::ServoMove* servo)
    {
        p->set(path_ + "MMF_MAX_BUF", servo->mmf_max_buf);
        p->set(path_ + "MMF_KP", servo->mmf_kp);
        p->set(path_ + "MMF_KV", servo->mmf_kv);
        p->set(path_ + "MMF_KA", servo->mmf_ka);
    }
    void get_mmf(Util::IniParser* p, AgConfig::ServoMove* servo)
    {
        p->get(path_ + "MMF_MAX_BUF", servo->mmf_max_buf, 0, 0, 100);
        p->get(path_ + "MMF_KP", servo->mmf_kp, 0, 0, 100);
        p->get(path_ + "MMF_KV", servo->mmf_kv, 0, 0, 100);
        p->get(path_ + "MMF_KA", servo->mmf_ka, 0, 0, 100);
    }
    void set_nlf(Util::IniParser* p, AgConfig::ServoMove* servo)
    {
        p->set(path_ + "NLF_MAX_VP", servo->nlf_vp);
        p->set(path_ + "NLF_MAX_AP", servo->nlf_ap);
        p->set(path_ + "NLF_MAX_JP", servo->nlf_jp);
        p->set(path_ + "NLF_MAX_VR", servo->nlf_vr);
        p->set(path_ + "NLF_MAX_AR", servo->nlf_ar);
        p->set(path_ + "NLF_MAX_JR", servo->nlf_jr);
    }
    void get_nlf(Util::IniParser* p, AgConfig::ServoMove* servo)
    {
        p->get(path_ + "NLF_MAX_VP", servo->nlf_vp, 0, 0, 100000);
        p->get(path_ + "NLF_MAX_AP", servo->nlf_ap, 0, 0, 100000);
        p->get(path_ + "NLF_MAX_JP", servo->nlf_jp, 0, 0, 100000);
        p->get(path_ + "NLF_MAX_VR", servo->nlf_vr, 0, 0, 100000);
        p->get(path_ + "NLF_MAX_AR", servo->nlf_ar, 0, 0, 100000);
        p->get(path_ + "NLF_MAX_JR", servo->nlf_jr, 0, 0, 100000);
    }
    void get_lpf(Util::IniParser* p, AgConfig::ServoMove* servo) { p->get(path_ + "LPF_CUTOFF_FREQ", servo->lpf_cf, 0, 0, 100); }
    void set_lpf(Util::IniParser* p, AgConfig::ServoMove* servo) { p->set(path_ + "LPF_CUTOFF_FREQ", servo->lpf_cf); }
};

class ConfigJoint : public ConfigItem
{
public:
    ConfigJoint(AgConfig::JointLimit* value) : ConfigItem("JOINT.", (void*)value){};
    void set_value(Util::IniParser* p)
    {
        auto j = (AgConfig::JointLimit*)value_;
        p->set(path_ + "JOINT_MAX_POS_LIMIT", j->max.vector_value());
        p->set(path_ + "JOINT_MIN_POS_LIMIT", j->min.vector_value());
        p->set(path_ + "JOINT_MAX_VEL", j->vel.vector_value());
        p->set(path_ + "JOINT_MAX_ACC", j->acc.vector_value());
        p->set(path_ + "JOINT_ERR_LIMIT", j->err_limit.vector_value());
        p->set(path_ + "JOINT_MAX_JERK", j->jerk);
    }
    void get_value(Util::IniParser* p)
    {
        auto j = (AgConfig::JointLimit*)value_;
        get_robjoint(p, path_ + "JOINT_MAX_POS_LIMIT", &j->max, 360, 0, 3600);
        get_robjoint(p, path_ + "JOINT_MIN_POS_LIMIT", &j->min, -360, -3600, 0);
        get_robjoint(p, path_ + "JOINT_MAX_VEL", &j->vel, 180, 0, 1000);
        get_robjoint(p, path_ + "JOINT_MAX_ACC", &j->acc, 720, 0, 10000);
        get_robjoint(p, path_ + "JOINT_ERR_LIMIT", &j->err_limit, 80, 0, 500);
        p->get(path_ + "JOINT_MAX_JERK", j->jerk, 0, 0, 500000);
    }
};

class ConfigStop : public ConfigItem
{
public:
    ConfigStop(AgConfig::Stop* value) : ConfigItem("STOP.", (void*)value) {}
    void set_value(Util::IniParser* p)
    {
        auto stop = (AgConfig::Stop*)value_;
        p->set(path_ + "CAT1_STOP_TRAJ_ACC", std::vector<double>({stop->cat1_acc.trans, stop->cat1_acc.ori}));
        p->set(path_ + "CAT2_STOP_TRAJ_ACC", std::vector<double>({stop->cat2_acc.trans, stop->cat2_acc.ori}));
        p->set(path_ + "CAT1_STOP_JOINT_ACC", stop->cat1_joint_acc);
        p->set(path_ + "CAT2_STOP_JOINT_ACC", stop->cat2_joint_acc);
    }
    void get_value(Util::IniParser* p)
    {
        auto stop = (AgConfig::Stop*)value_;
        get_tcp1d(p, path_ + "CAT1_STOP_TRAJ_ACC", &stop->cat1_acc, {3500, 180}, 0, 10000);
        get_tcp1d(p, path_ + "CAT2_STOP_TRAJ_ACC", &stop->cat2_acc, {3500, 180}, 0, 10000);
        p->get(path_ + "CAT1_STOP_JOINT_ACC", stop->cat1_joint_acc, 720, 0, 10000);
        p->get(path_ + "CAT2_STOP_JOINT_ACC", stop->cat2_joint_acc, 720, 0, 10000);
    }
};
class ConfigDefaultPose : public ConfigItem
{
public:
    ConfigDefaultPose(AgConfig::DefaultJointPos* value) : ConfigItem("DEFAULTPOSE.", (void*)value) {}
    void set_value(Util::IniParser* p)
    {
        auto pose = (AgConfig::DefaultJointPos*)value_;
        p->set(path_ + "SHRUNKENPOSE", pose->shrunken.vector_value());
        p->set(path_ + "STRETCHEDPOSE", pose->stretched.vector_value());
        p->set(path_ + "INITIALPOSE", pose->initial.vector_value());
        p->set(path_ + "POSETOL", pose->tol);
        p->set(path_ + "HOME_VEL", pose->vel_scale);
    }
    void get_value(Util::IniParser* p)
    {
        auto pose = (AgConfig::DefaultJointPos*)value_;
        get_robjoint(p, path_ + "SHRUNKENPOSE", &pose->shrunken, 0, -3600, 3600);
        get_robjoint(p, path_ + "STRETCHEDPOSE", &pose->stretched, 0, -3600, 3600);
        get_robjoint(p, path_ + "INITIALPOSE", &pose->initial, 0, -3600, 3600);
        p->get(path_ + "POSETOL", pose->tol, 0.1, 0.0001, 10);
        p->get(path_ + "HOME_VEL", pose->vel_scale, 30.0, 0.1, 100);
    }
};
class ConfigForceControl : public ConfigItem
{
public:
    ConfigForceControl(FCAxisPara* value) : ConfigItem("FORCECONTROL.", (void*)value) {}
    void set_value(Util::IniParser* p)
    {
        auto fc_para = (FCAxisPara*)value_;
        p->set(path_ + "HZ", fc_para->torque_sensor_filter);
        set_sensor_bias(p, "SENSORBIAS", fc_para->torque_sensor_bias);
        set_sensor_limit(p, "TORQUE_SENSOR_LIMIT", fc_para->torque_sensor_limit);

        set_fc_config(p, "ADMITCTRLCONFIG_1", fc_para->admit_fc_config[0]);
        set_fc_config(p, "ADMITCTRLCONFIG_2", fc_para->admit_fc_config[1]);
        set_fc_config(p, "ADMITCTRLCONFIG_3", fc_para->admit_fc_config[2]);
        set_fc_config(p, "ADMITCTRLCONFIG_4", fc_para->admit_fc_config[3]);
        set_fc_config(p, "ADMITCTRLCONFIG_5", fc_para->admit_fc_config[4]);
        set_fc_config(p, "ADMITCTRLCONFIG_6", fc_para->admit_fc_config[5]);
        p->set(path_ + "ADMITCTRLFRAME", fc_para->admit_fc_frame);

        set_fc_config(p, "ADMITDRAGCONFIG_1", fc_para->admit_drag_config[0]);
        set_fc_config(p, "ADMITDRAGCONFIG_2", fc_para->admit_drag_config[1]);
        set_fc_config(p, "ADMITDRAGCONFIG_3", fc_para->admit_drag_config[2]);
        set_fc_config(p, "ADMITDRAGCONFIG_4", fc_para->admit_drag_config[3]);
        set_fc_config(p, "ADMITDRAGCONFIG_5", fc_para->admit_drag_config[4]);
        set_fc_config(p, "ADMITDRAGCONFIG_6", fc_para->admit_drag_config[5]);
        p->set(path_ + "ADMITDRAGFRAME", fc_para->admit_drag_frame);

        p->set(path_ + "COMPLIANT_TYPE", fc_para->compliant_type);
        p->set(path_ + "COMPLIANT_ENABLE", fc_para->compliant_enable);

        set_dead_zone(p, "DRAG_DEAD_ZONE", fc_para->drag_dead_zone);
        p->set(path_ + "ADMIT_DRAG_JSENSITIVITY", fc_para->admit_drag_jsensitivity);
        p->set(path_ + "ADMIT_DRAG_SIGULAR_THRESHOLD", fc_para->admit_drag_sigular_threshold);
        p->set(path_ + "CONSTANT_FORCE_CTRL_TOL", fc_para->constant_force_ctrl_tol);
        p->set(path_ + "CONSTANT_TORQUE_CTRL_TOL", fc_para->constant_torque_ctrl_tol);
        p->set(path_ + "NON_CONTACT_VEL_LIMIT", fc_para->non_contact_vel_limit);
        p->set(path_ + "NON_CONTACT_ANGULAR_VEL_LIMIT", fc_para->non_contact_angular_vel_limit);
        p->set(path_ + "FORCE_CTRL_VEL_LIMIT", fc_para->force_ctrl_vel_limit);
        p->set(path_ + "FORCE_CTRL_ANGULAR_VEL_LIMIT", fc_para->force_ctrl_angular_vel_limit);
        p->set(path_ + "FORCE_REF_POINT", fc_para->force_ref_point);
        set_contact_force_threshold(p, "CONTACT_FORCE_THRESHOLD", fc_para->contact_force_threshold);
    }
    void get_value(Util::IniParser* p)
    {
        auto fc_para = (FCAxisPara*)value_;
        p->get(path_ + "HZ", fc_para->torque_sensor_filter, 0, 0, 1000);

        get_sensor_bias(p, "SENSORBIAS", fc_para->torque_sensor_bias);
        get_sensor_limit(p, "TORQUE_SENSOR_LIMIT", fc_para->torque_sensor_limit);

        get_fc_config(p, "ADMITCTRLCONFIG_1", fc_para->admit_fc_config[0]);
        get_fc_config(p, "ADMITCTRLCONFIG_2", fc_para->admit_fc_config[1]);
        get_fc_config(p, "ADMITCTRLCONFIG_3", fc_para->admit_fc_config[2]);
        get_fc_config(p, "ADMITCTRLCONFIG_4", fc_para->admit_fc_config[3]);
        get_fc_config(p, "ADMITCTRLCONFIG_5", fc_para->admit_fc_config[4]);
        get_fc_config(p, "ADMITCTRLCONFIG_6", fc_para->admit_fc_config[5]);
        int frame;
        p->get(path_ + "ADMITCTRLFRAME", frame, 0, 0, 1);
        fc_para->admit_fc_frame = (FCAxisPara::FRAME)frame;
        get_fc_config(p, "ADMITDRAGCONFIG_1", fc_para->admit_drag_config[0]);
        get_fc_config(p, "ADMITDRAGCONFIG_2", fc_para->admit_drag_config[1]);
        get_fc_config(p, "ADMITDRAGCONFIG_3", fc_para->admit_drag_config[2]);
        get_fc_config(p, "ADMITDRAGCONFIG_4", fc_para->admit_drag_config[3]);
        get_fc_config(p, "ADMITDRAGCONFIG_5", fc_para->admit_drag_config[4]);
        get_fc_config(p, "ADMITDRAGCONFIG_6", fc_para->admit_drag_config[5]);
        p->get(path_ + "ADMITDRAGFRAME", frame, 0, 0, 1);
        fc_para->admit_drag_frame = (FCAxisPara::FRAME)frame;
        p->get(path_ + "COMPLIANT_TYPE", fc_para->compliant_type, 0, 0, 1);
        p->get(path_ + "COMPLIANT_ENABLE", fc_para->compliant_enable, 1, 0, 1);

        get_dead_zone(p, "DRAG_DEAD_ZONE", fc_para->drag_dead_zone);
        p->get(path_ + "ADMIT_DRAG_JSENSITIVITY", fc_para->admit_drag_jsensitivity, 0, 0, 1000);
        p->get(path_ + "ADMIT_DRAG_SIGULAR_THRESHOLD", fc_para->admit_drag_sigular_threshold, 2, 0, 1000);
        p->get(path_ + "CONSTANT_FORCE_CTRL_TOL", fc_para->constant_force_ctrl_tol, 0.0, 0.0, 1000);
        p->get(path_ + "CONSTANT_TORQUE_CTRL_TOL", fc_para->constant_torque_ctrl_tol, 0.0, 0.0, 1000);
        p->get(path_ + "NON_CONTACT_VEL_LIMIT", fc_para->non_contact_vel_limit, 10.0, 0.0, 1000);
        p->get(path_ + "NON_CONTACT_ANGULAR_VEL_LIMIT", fc_para->non_contact_angular_vel_limit, 1.0, 0.0, 1000);
        p->get(path_ + "FORCE_CTRL_VEL_LIMIT", fc_para->force_ctrl_vel_limit, 1000.0, 0.0, 1000);
        p->get(path_ + "FORCE_CTRL_ANGULAR_VEL_LIMIT", fc_para->force_ctrl_angular_vel_limit, 120.0, 0.0, 1000);
        p->get(path_ + "FORCE_REF_POINT", fc_para->force_ref_point, 0, 0, 1);
        get_contact_force_threshold(p, "CONTACT_FORCE_THRESHOLD", fc_para->contact_force_threshold);
    }

private:
    void get_fc_config(Util::IniParser* p, std::string tag, FtAxisConfig& config)
    {
        std::vector<double> res(6, 0);
        std::vector<double> def(6, 0);
        p->get(path_ + tag, res, def, 0, 1000);
        config.enableFt = res[1];
        config.enableNTrack = res[2];
        config.ftUser = res[3];
        config.ftReboundFK = res[4];
        config.ftConstant = res[5];
    }
    void set_fc_config(Util::IniParser* p, std::string tag, FtAxisConfig& config)
    {
        std::vector<double> res = {
            (double)tag.back(), (double)config.enableFt, (double)config.enableNTrack, config.ftUser, config.ftConstant, config.ftReboundFK};
        p->set(path_ + tag, res);
    }
    void get_sensor_bias(Util::IniParser* p, std::string tag, double* val)
    {
        std::vector<double> res(6, 0);
        std::vector<double> def(6, 0);
        p->get(path_ + tag, res, def, -1000, 1000);
        for (int i = 0; i < 6; i++) val[i] = res[i];
    }
    void set_sensor_bias(Util::IniParser* p, std::string tag, const double* val)
    {
        std::vector<double> res = {val[0], val[1], val[2], val[3], val[4], val[5]};
        p->set(path_ + tag, res);
    }
    void get_sensor_limit(Util::IniParser* p, std::string tag, double* val)
    {
        std::vector<double> res(6, 0);
        std::vector<double> def(6, 0);
        p->get(path_ + tag, res, def, 0, 1000);
        for (int i = 0; i < 6; i++) val[i] = res[i];
    }
    void set_sensor_limit(Util::IniParser* p, std::string tag, const double* val)
    {
        std::vector<double> res = {val[0], val[1], val[2], val[3], val[4], val[5]};
        p->set(path_ + tag, res);
    }
    void get_contact_force_threshold(Util::IniParser* p, std::string tag, double* val)
    {
        std::vector<double> res(6, 0);
        std::vector<double> def(6, 0);
        p->get(path_ + tag, res, def, 0, 1000);
        for (int i = 0; i < 6; i++) val[i] = res[i];
    }
    void set_contact_force_threshold(Util::IniParser* p, std::string tag, const double* val)
    {
        std::vector<double> res = {val[0], val[1], val[2], val[3], val[4], val[5]};
        p->set(path_ + tag, res);
    }
    void get_dead_zone(Util::IniParser* p, std::string tag, double* val)
    {
        std::vector<double> res(6, 0);
        std::vector<double> def(6, 0);
        p->get(path_ + tag, res, def, 0, 1000);
        for (int i = 0; i < 6; i++) val[i] = res[i];
    }
    void set_dead_zone(Util::IniParser* p, std::string tag, const double* val)
    {
        std::vector<double> res = {val[0], val[1], val[2], val[3], val[4], val[5]};
        p->set(path_ + tag, res);
    }
};

class ConfigSafetyCollision : public ConfigItem
{
public:
    ConfigSafetyCollision(AgConfig::SafetyCollision* value) : ConfigItem("MOTSAFE_COLLISION.", (void*)value){};
    virtual void set_value(Util::IniParser* p)
    {
        auto colli = (AgConfig::SafetyCollision*)value_;
        p->set(path_ + "REACTION_TYPE", colli->reaction_type);
        p->set(path_ + "REBOUND_ANGLE", colli->rebound_angle);
        p->set(path_ + "REBOUND_LENGTH", colli->rebound_length);
        p->set(path_ + "REBOUND_J_ACC", colli->rebound_j_acc);
        p->set(path_ + "REBOUND_J_VEL", colli->rebound_j_vel);
        p->set(path_ + "REBOUND_L_ACC", colli->rebound_l_acc);
        p->set(path_ + "REBOUND_L_VEL", colli->rebound_l_vel);
        p->set(path_ + "COLLISION_METHOD", colli->collision_method);
        p->set(path_ + "GM_FILTER", colli->gm_filter);
        p->set(path_ + "GM_THRESHOLD", colli->gm_threshold);
        p->set(path_ + "JOINT_SENSITIVITY", colli->joint_sensitivity.vector_value());
        p->set(path_ + "FORCE_LIMIT_VALUE", colli->force_limit_value);
        p->set(path_ + "COLLISION_DETECTION_ENABLE", colli->collision_detection_enable);
        p->set(path_ + "CLSN_SENSITIVITY", colli->clsn_sensitivity);
    }
    virtual void get_value(Util::IniParser* p)
    {
        auto colli = (AgConfig::SafetyCollision*)value_;
        p->get(path_ + "REACTION_TYPE", colli->reaction_type, 0, 0, 1);
        p->get(path_ + "REBOUND_ANGLE", colli->rebound_angle, 3, 0, 10);
        p->get(path_ + "REBOUND_LENGTH", colli->rebound_length, 30, 0, 100);
        p->get(path_ + "REBOUND_J_ACC", colli->rebound_j_acc, 720, 0, 10000);
        p->get(path_ + "REBOUND_J_VEL", colli->rebound_j_vel, 180, 0, 10000);
        p->get(path_ + "REBOUND_L_ACC", colli->rebound_l_acc, 2000, 0, 10000);
        p->get(path_ + "REBOUND_L_VEL", colli->rebound_l_vel, 2000, 0, 10000);
        p->get(path_ + "COLLISION_METHOD", colli->collision_method, 0, 0, 10);
        p->get(path_ + "GM_FILTER", colli->gm_filter, 0, 0, 1000);
        p->get(path_ + "GM_THRESHOLD", colli->gm_threshold, {1.0, 1.05, 1.1, 1.3, 1.5}, 0, 100);
        get_robjoint(p, path_ + "JOINT_SENSITIVITY", &colli->joint_sensitivity, 50, 0, 100);
        p->get(path_ + "FORCE_LIMIT_VALUE", colli->force_limit_value, 0, 0, 200);
        p->get(path_ + "COLLISION_DETECTION_ENABLE", colli->collision_detection_enable, 1, 0, 1);
        p->get(path_ + "CLSN_SENSITIVITY", colli->clsn_sensitivity, 1, 0, 1);
    }
};

class ConfigRobotAxisCfg : public ConfigItem
{
public:
    ConfigRobotAxisCfg(AgConfig::RobotAxisCfg* value) : ConfigItem("ROBOT.", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotAxisCfg*)value_;
        p->get(path_ + "AXIS_ID", cfg->axis_start_id, 0, 0, 100);
        p->get(path_ + "BUS_CHN", cfg->bus_chn, "robbus0");
        p->get(path_ + "AXIS_NUM", cfg->axis_num, 6, 0, 7);
        std::vector<int32_t> default_bus_devid;
        for (int i = 0; i < cfg->axis_num; i++) default_bus_devid.push_back(i);
        p->get(path_ + "BUS_DEVID", cfg->bus_devid, default_bus_devid);
    }
};

class ConfigRobotMotorCfg : public ConfigItem
{
public:
    ConfigRobotMotorCfg(AgConfig::RobotJointPara* value) : ConfigItem("MOTOR.", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotJointPara*)value_;
        get_robjoint(p, path_ + "MOTOR_RATED_TORQ", &cfg->motor_tor, 0, 0, 1000);
        get_robjoint(p, path_ + "MOTOR_RATED_TRANS_RATIO", &cfg->gear_radio, 0, 0, 1000);
    }
};

class ConfigRobotJointCfg : public ConfigItem
{
public:
    ConfigRobotJointCfg(AgConfig::RobotJointPara* value) : ConfigItem("JOINT.", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotJointPara*)value_;
        get_robjoint(p, path_ + "JOINT_OFFSET", &cfg->offset, 0, -1000, 1000);
        get_robjoint(p, path_ + "JOINT_DIR", &cfg->dir, 1, -1, 1);
        get_robjoint(p, path_ + "JOINT_MAX_VEL", &cfg->default_joint_vel_limit, 60, 0, 10000);
        get_robjoint(p, path_ + "JOINT_MAX_ACC", &cfg->default_joint_acc_limit, 100, 0, 10000);
        get_robjoint(p, path_ + "JOINT_MIN_POS_LIMIT", &cfg->default_joint_min_pos, -360, -720, 720);  // #TODO 无限旋转关节？
        get_robjoint(p, path_ + "JOINT_MAX_POS_LIMIT", &cfg->default_joint_max_pos, 360, -720, 720);
        get_robjoint(p, path_ + "JOINT_ERR_LIMIT", &cfg->default_joint_err_limit, 80, 0, 100);
    }
};

class ConfigRobotDH : public ConfigItem
{
public:
    ConfigRobotDH(AgConfig::RobotDefaultDH* value) : ConfigItem("DHPARAM.", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotDefaultDH*)value_;
        std::vector<double> fDHParam;
        std::vector<double> default_fDHParam = {340.0, 0.0, 400.0, 0.0, 400.0, 0.0, 111.0, 0,   0,  0,   0,  0,   0,  0,
                                                0,     0,   0,     0,   0,     0,   0,     -90, 90, -90, 90, -90, 90, 0};  //默认使用AT2023的DH参数
        p->get(path_ + "DH_PARAM", fDHParam, default_fDHParam, -65535.0, 65535.0);
        for (size_t i = 0; i < fDHParam.size(); i++) { cfg->dh[i] = fDHParam[i]; }
    }
};

class ConfigRobotDYN : public ConfigItem
{
public:
    ConfigRobotDYN(AgConfig::RobotDefaultDYN* value) : ConfigItem("DYNAMICS.", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotDefaultDYN*)value_;
        std::vector<double> fDYNParam;
        std::vector<double> default_fDYNParam(14 * MAX_AXIS, 0);
        p->get(path_ + "KINETIC_PARAM", fDYNParam, default_fDYNParam, -65535.0, 65535.0);

        std::vector<double> traj;
        std::vector<double> default_traj(7 * 4 * MAX_AXIS, 0);
        p->get(path_ + "TRAJ_PARA", traj, default_traj, -65535.0, 65535.0);

        for (size_t i = 0; i < traj.size(); i++) { cfg->dyn_traj_para[i] = traj[i]; }
    }
};

class ConfigRobotSafeCustomSet : public ConfigItem
{
public:
    ConfigRobotSafeCustomSet(AgConfig::RobotDefaultSafety* value) : ConfigItem("", (void*)value) {}
    void set_value(Util::IniParser* p) {}
    void get_value(Util::IniParser* p)
    {
        auto cfg = (AgConfig::RobotDefaultSafety*)value_;
        std::string section[4] = {"NOLIMITSAFETYSETTING", "LOOSESAFETYSETTING", "MEDIUMSAFETYSETTING", "STRICTSAFETYSETTING"};
        for (int i = 0; i < 4; ++i)
        {
            p->get(section[i] + ".MOMENTUM_LIMIT", cfg[i].momentum_limit, 70.0, 0, __DBL_MAX__);
            p->get(section[i] + ".TCP_VEL_LIMIT", cfg[i].tcp_vel_limit, 1620.0, 0, __DBL_MAX__);
            p->get(section[i] + ".ROBOT_POWER_LIMIT", cfg[i].robot_power_limit, 1500.0, 0, __DBL_MAX__);
            p->get(section[i] + ".STOPPING_DIST", cfg[i].stopping_dist, 500.0, 0, __DBL_MAX__);
            p->get(section[i] + ".STOPPING_TIME", cfg[i].stopping_time, 0.4, 0, __DBL_MAX__);
        }
    }
};

axisgroup::AgConfig::RobotConfig& axisgroup::AgConfig::get_robot_config() { return config_; }

AgConfig::AgConfig()
{
    reset();
    append_item(std::make_shared<ConfigInt32Type>("PAYLOAD.WORK_PAYLOAD_ID", &config_.work_payload_id));
    append_item(std::make_shared<ConfigInt32Type>("PAYLOAD.SENSOR_PAYLOAD_ID", &config_.sensor_payload_id));
    append_item(std::make_shared<ConfigInt32Type>("BASEOFFSET.FRAME_ID", &config_.base_frame_id));
    append_item(std::make_shared<ConfigInt32Type>("MANUEL.TCP_ID", &config_.tool_frame_id));
    append_item(std::make_shared<ConfigInt32Type>("MANUEL.USR_FRAME_ID", &config_.user_frame_id));
    append_item(std::make_shared<ConfigTraj>(&config_.traj));
    append_item(std::make_shared<ConfigJoint>(&config_.joint_limit));
    append_item(std::make_shared<ConfigJog>(&config_.jog));
    append_item(std::make_shared<ConfigStop>(&config_.stop));
    append_item(std::make_shared<ConfigServoMove>(&config_.servo_move));
    append_item(std::make_shared<ConfigDefaultPose>(&config_.default_pose));
    append_item(std::make_shared<ConfigSafetyCollision>(&config_.robot_safety.collision));
    append_item(std::make_shared<ConfigForceControl>(&config_.force_control));

    model_items_.push_back(std::make_shared<ConfigRobotAxisCfg>(&config_.robot_axis_cfg));
    model_items_.push_back(std::make_shared<ConfigRobotMotorCfg>(&config_.robot_model.joint_para));
    model_items_.push_back(std::make_shared<ConfigRobotJointCfg>(&config_.robot_model.joint_para));
    model_items_.push_back(std::make_shared<ConfigRobotDH>(&config_.robot_model.dh));
    model_items_.push_back(std::make_shared<ConfigRobotDYN>(&config_.robot_model.dyn));
    model_items_.push_back(std::make_shared<ConfigRobotSafeCustomSet>(config_.robot_model.custom_setting));
    model_items_.push_back(std::make_shared<ConfigTraj>(&config_.robot_model.traj));
    model_items_.push_back(std::make_shared<ConfigDefaultPose>(&config_.robot_model.default_pose));
}

void AgConfig::reset()
{
    memset(&config_.traj, 0, sizeof(config_.traj));
    memset(&config_.joint_limit, 0, sizeof(config_.joint_limit));
    memset(&config_.jog, 0, sizeof(config_.jog));
    memset(&config_.stop, 0, sizeof(config_.stop));
    memset(&config_.servo_move, 0, sizeof(config_.servo_move));
    memset(&config_.base_frame_id, 0, sizeof(config_.base_frame_id));
    memset(&config_.default_pose, 0, sizeof(config_.default_pose));
    memset(&config_.robot_axis_cfg, 0, sizeof(config_.robot_axis_cfg));
}

int AgConfig::append_item(std::shared_ptr<ConfigItem> item)
{
    user_items_.push_back(item);
    return 0;
}

std::string axisgroup::AgConfig::find_model_config(uint32_t serial_num)
{
    auto serial = serial_num / 10000;
    std::string file = "." + std::to_string(serial) + ".ini";
    std::string path = ZUC_DEFAULT_CONFIG_DIR + std::string("/rob/") + file;
    Util::IniParser parser(path.c_str());
    if (false == parser.is_valid())
    {
        return "";
    }
    //TODO:
    return path;
}

int AgConfig::load_model_config(const std::string& filepath)
{
    Util::IniParser parser(filepath.c_str());
    if (false == parser.is_valid())
    {
        return -1;
    }
    for (auto& cfg : model_items_) { cfg->get_value(&parser); }

    return 0;
}

int AgConfig::load_usr_config(std::string usr_cfg_file)
{
    usr_cfg_ = usr_cfg_file;
    Util::IniParser parser(usr_cfg_.c_str());
    if (false == parser.is_valid())
    {
        return -1;
    }
    for (auto& cfg : user_items_) { cfg->get_value(&parser); }

    return 0;
}

void axisgroup::AgConfig::write_serial_num(const std::string& usr_cfg_file, int serial)
{
    Util::IniParser usr_parser(usr_cfg_file.c_str());
    usr_parser.set("ROBOT.SERIAL_NUM", serial);
}

void AgConfig::config_dh(int joint_num)
{
    for (int i = 0; i < joint_num; i++)
    {
        config_.robot_model.dh.robot_dh.d[i] = config_.robot_model.dh.dh[i + 0 * joint_num];
        config_.robot_model.dh.robot_dh.a[i] = config_.robot_model.dh.dh[i + 1 * joint_num];
        config_.robot_model.dh.robot_dh.joint_homeoff[i] = config_.robot_model.dh.dh[i + 2 * joint_num];  // 标定零位偏差
        config_.robot_model.dh.robot_dh.alpha[i] = config_.robot_model.dh.dh[i + 3 * joint_num];
    }
}

bool axisgroup::AgConfig::fit_to_robot_config()
{
    bool cfg_changed = false;
    // 关节限位
    for (size_t i = 0; i < config_.joint_limit.min.MAX_AXIS; i++)
    {
        if (config_.joint_limit.min[i] < config_.robot_model.joint_para.default_joint_min_pos[i])
        {
            config_.joint_limit.min[i] = config_.robot_model.joint_para.default_joint_min_pos[i];
            cfg_changed = true;
        }
        if (config_.joint_limit.max[i] > config_.robot_model.joint_para.default_joint_max_pos[i])
        {
            config_.joint_limit.max[i] = config_.robot_model.joint_para.default_joint_max_pos[i];
            cfg_changed = true;
        }
    }
    // 关节速度
    for (size_t i = 0; i < config_.joint_limit.vel.MAX_AXIS; i++)
    {
        if (config_.joint_limit.vel[i] > config_.robot_model.joint_para.default_joint_vel_limit[i])
        {
            config_.joint_limit.vel[i] = config_.robot_model.joint_para.default_joint_vel_limit[i];
            cfg_changed = true;
        }
    }
    // 关节加速度
    for (size_t i = 0; i < config_.joint_limit.acc.MAX_AXIS; i++)
    {
        if (config_.joint_limit.acc[i] > config_.robot_model.joint_para.default_joint_acc_limit[i])
        {
            config_.joint_limit.acc[i] = config_.robot_model.joint_para.default_joint_acc_limit[i];
            cfg_changed = true;
        }
    }
    return cfg_changed;
}

int AgConfig::write_usr_config_to_file()
{
    Util::IniParser parser(usr_cfg_.c_str());
    for (auto& cfg : user_items_) { cfg->set_value(&parser); }
    return 0;
}
